%弹簧阻尼系统
%% 代码主题
% 1.两种方式计算反馈增益F，并且对两种方式计算的F值进行使用
% 方法1是在程序中硬算，方法2是调用<F1_LQR_Gain.m>函数
% 2.调整输入的阶数，1阶和2阶时候的使用，调整B，u0，R
%%
%%代码的实现
clear;
close;
A = [0 1; -1 -0.5];
B = [0;1];%输入为1阶
% B = [0 0.2;-0.1 0.5];%输入为2阶
C = [1 0];
D = 0;
p = size(B,2);%B的列数
n = size(A,1);
Ts = 0.1;%步长
%连续系统转成离散系统
sys_d = c2d(ss(A,B,C,D),Ts);
%提前离散系统A矩阵
A = sys_d.a;
%提前离散系统A矩阵
B = sys_d.b;
%%%%%%%%%%%%%系统初始化%%%%%%%%%%%
x0 = [1;0];
x = x0;
%输入初始化
u0 = 0;%输入为1阶
% u0 = [0;0];%输入为2阶
u = u0;
%%%%%%%%%%%%%%%%%%
%定义系统运行步数
k_steps = 100;
x_history = zeros(n,k_steps);
x_history(:,1) = x;

u_history = zeros(p,k_steps);
u_history(:,1) = u;
Q = [1 0; 0 1];
S = [1 0; 0 1];
R=0.1;
% R= [0.1 0; 0 0.1];%输入为2阶
N = k_steps;
P_k = S;
% 反馈gain(F(N-k))
% 方法1.1
for k = 1:N
    F = inv(R+B'*P_k*B)*B'*P_k*A;
    P_k = (A-B*F)'*P_k*(A-B*F)+(F)'*R*(F)+Q;
    if k==1
        F_N=F;
    else
        F_N=[F;F_N];
    end
end
% 方法2.1
[F] = F1_LQR_Gain(A,B,Q,R,S);

for k = 1:k_steps
    %计算最优的u
%     方法1.1
    u = -F_N((k-1)*p+1:k*p,:)*x;
%   方法2.2
%     u = -F*x;
%     后边程序相同
    x=A*x+B*u;
    x_history(:,k+1)=x;
    u_history(:,k)=u;
end
%% 制图
subplot(2,1,1);
for i=1:n
    plot(x_history(i,:));
    hold;   
end
legend(num2str((1:n)','x %d'));
xlim([1,k_steps]);
grid on;

subplot(2,1,2);
for i=1:p
    stairs(u_history(i,:));
    hold;   
end
legend(num2str((1:p)','u %d'));
xlim([1,k_steps]);
grid on;






